// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PoseFeedbackState.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Hardware.DAC
{
    using System;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Algorithms;
    using Microsoft.Robotics.Numerics;
    using MicrosoftResearch.Infer.Distributions;    

    /// <summary>
    /// This class represents takes the absolute poses of the robot and exposes them as relative delta poses.
    /// It can also apply that delta pose to an arbitrary previous pose in that coordinate system with or without noise.
    /// </summary>
    [DataContract]
    public class PoseFeedbackState : IDriveState
    {
        /// <summary>
        /// Odometry noise model params.
        /// </summary>
        [DataMember]
        private PoseNoiseModelParams noiseParams;

        /// <summary>
        /// Absolute Position of the robot in x (meters), y (meters) and heading (radians)
        /// </summary>
        [DataMember]
        private Pose2D position;

        /// <summary>
        /// Initializes a new instance of the <see cref="PoseFeedbackState"/> class.
        /// </summary>
        /// <param name="pose">Absolute Pose in X (meters), Y (meters), Heading (Radians)</param>
        /// <param name="noiseParams">Pose noise parameters</param>
        public PoseFeedbackState(Pose2D pose, PoseNoiseModelParams noiseParams)            
        {
            this.position = pose;
            this.noiseParams = noiseParams;
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="PoseFeedbackState"/> class.
        /// </summary>
        /// <param name="that">Prototype for constructing this</param>
        public PoseFeedbackState(PoseFeedbackState that)
        {
            this.noiseParams = that.noiseParams;
            this.position = that.position;
        }

        /// <summary>
        /// Interpolates the between the start and end states for the desiredTime.
        /// </summary>
        /// <param name="startTime">The start time for interpolation</param>
        /// <param name="endTime">The end time for interpolation</param>
        /// <param name="desiredTime">Desired time for interpolation. This must be between (inclusive) start and end times</param>
        /// <param name="start">Start value</param>
        /// <param name="end">End value</param>    
        /// <returns>The linear interpolated value at the desired time</returns>
        public static PoseFeedbackState Interpolate(long startTime, long endTime, long desiredTime, PoseFeedbackState start, PoseFeedbackState end)
        {
            PoseFeedbackState interpolated = new PoseFeedbackState(start);
            interpolated.position.X = Interpolator.LinearInterpolate(startTime, endTime, desiredTime, start.position.X, end.position.X);
            interpolated.position.Y = Interpolator.LinearInterpolate(startTime, endTime, desiredTime, start.position.Y, end.position.Y);
            
            return interpolated;
        }

        /// <summary>
        /// Gets the mean delta position (deltaX, deltaY, deltaHeading) from the given previous state.
        /// </summary>
        /// <param name="previousState">Previous differential state</param>
        /// <returns>The mean delta position (deltaX, deltaY, deltaHeading) from the given previous state. </returns>
        public Pose2D GetMeanDeltaPose(IDriveState previousState)
        {
            double fitness;
            return this.GetMeanDeltaPose(previousState, out fitness);
        }

        /// <summary>
        /// Gets the mean delta position (deltaX, deltaY, deltaHeading) from the given previous state.
        /// </summary>
        /// <param name="previousState">Previous differential state</param>
        /// <param name="fitness">Fitness of the current sample's wheel and gyro heading</param>
        /// <returns>The mean delta position (deltaX, deltaY, deltaHeading) from the given previous state. </returns>
        public Pose2D GetMeanDeltaPose(IDriveState previousState, out double fitness)
        {
            PoseFeedbackState previousPoseFeedbackState = previousState as PoseFeedbackState;
            if (previousPoseFeedbackState == null)
            {
                throw new System.ArgumentException(string.Format("Previous drive feeback type {0} is different from current drive feedback type {1}", previousState.GetType(), this.GetType()));
            }

            return this.GetDeltaPoseWithNoise(previousPoseFeedbackState, false, out fitness);
        }

        /// <summary>
        /// Samples the delta pose (deltaX, deltaY, deltaHeading) from the given previous state.
        /// </summary>
        /// <param name="previousState">Previous differential state</param>        
        /// <returns>The sampled delta position (deltaX, deltaY, deltaHeading) from the given previous state.</returns>
        public Pose2D SampleDeltaPose(IDriveState previousState)
        {
            double fitness;
            return this.SampleDeltaPose(previousState, out fitness);
        }

        /// <summary>
        /// Samples the delta pose (deltaX, deltaY, deltaHeading) from the given previous state.
        /// </summary>
        /// <param name="previousState">Previous differential state</param>
        /// <param name="fitness">Fitness of the current sample's wheel and gyro heading</param>
        /// <returns>The sampled delta position (deltaX, deltaY, deltaHeading) from the given previous state.</returns>
        public Pose2D SampleDeltaPose(IDriveState previousState, out double fitness)
        {
            PoseFeedbackState previousPoseFeedbackState = previousState as PoseFeedbackState;
            if (previousPoseFeedbackState == null)
            {
                throw new System.ArgumentException(string.Format("Previous drive feeback type {0} is different from current drive feedback type {1}", previousState.GetType(), this.GetType()));
            }

            return this.GetDeltaPoseWithNoise(previousPoseFeedbackState, !this.noiseParams.IgnoreNoiseParams, out fitness);
        }

        /// <summary>
        /// Gets the updated absolute pose (X, Y, Heading) from the given  previous pose and previous state.
        /// </summary>
        /// <param name="previousPose">Previous pose to update x (meters), y (meters) and heading (radians)</param>
        /// <param name="previousState">Previous differential state</param>        
        /// <returns>The mean delta position (deltaX, deltaY, deltaHeading) from the given previous state. </returns>
        public Pose2D GetUpdatedPose(Pose2D previousPose, IDriveState previousState)
        {
            double fitness;
            return this.GetUpdatedPose(previousPose, previousState, out fitness);
        }

        /// <summary>
        /// Gets the updated  absolute pose (X, Y, Heading) from the given  previous pose and previous state.
        /// </summary>
        /// <param name="previousPose">Previous pose to update x (meters), y (meters) and heading (radians)</param>
        /// <param name="previousState">Previous differential state</param>
        /// <param name="fitness">Fitness of the current sample's wheel and gyro heading</param>
        /// <returns>The mean delta position (deltaX, deltaY, deltaHeading) from the given previous state. </returns>
        public Pose2D GetUpdatedPose(Pose2D previousPose, IDriveState previousState, out double fitness)
        {
            Pose2D deltaPose = this.GetMeanDeltaPose(previousState, out fitness);
         
            Pose2D updatedPose = previousPose + deltaPose;

            ////Tracing.TraceOut.Info(Tracing.TraceContexts.Nav, "Mean Updated pose {0} delta pose {1}", updatedPose, deltaPose);
            return updatedPose;
        }

        /// <summary>
        /// Samples the pose (X, Y, Heading) from the given previous state
        /// </summary>
        /// <param name="previousPose">Previous pose to update x (meters), y (meters) and heading (radians)</param>
        /// <param name="previousState">Previous differential state</param>        
        /// <returns>The sampled delta position (deltaX, deltaY, deltaHeading) from the given previous state.</returns>
        public Pose2D SamplePose(Pose2D previousPose, IDriveState previousState)
        {
            double fitness;
            return this.SamplePose(previousPose, previousState, out fitness);
        }

        /// <summary>
        /// Samples the pose (X, Y, Heading) from the given previous state
        /// </summary>
        /// <param name="previousPose">Previous pose to update x (meters), y (meters) and heading (radians)</param>
        /// <param name="previousState">Previous differential state</param>
        /// <param name="fitness">Fitness of the current sample's wheel and gyro heading</param>
        /// <returns>The sampled delta position (deltaX, deltaY, deltaHeading) from the given previous state.</returns>
        public Pose2D SamplePose(Pose2D previousPose, IDriveState previousState, out double fitness)
        {
            Pose2D deltaPose = this.SampleDeltaPose(previousState, out fitness);
         
            Pose2D updatedPose = previousPose + deltaPose;            

            return updatedPose;
        }

        /// <summary>
        /// Gets the delta pose after applying noise if specified.
        /// </summary>
        /// <param name="previousState">The previous pose feedback state</param>        
        /// <param name="applyNoise">Indicates whether to apply the noise model</param>
        /// <param name="fitness">The fitness between the wheel heading delta and the external sensor (for example gyro) heading delta
        /// This is NaN if there is no external sensor information</param>        
        /// <returns>The position delta from previous pose</returns>
        private Pose2D GetDeltaPoseWithNoise(PoseFeedbackState previousState, bool applyNoise, out double fitness)
        {
            Pose2D poseDelta;
            
            double xDeltaOriginal = this.position.X - previousState.position.X;
            double yDeltaOriginal = this.position.Y - previousState.position.Y;
            double translationAngle = Math.Atan2(yDeltaOriginal, xDeltaOriginal);
            double translationDelta = Math.Sqrt(xDeltaOriginal * xDeltaOriginal + yDeltaOriginal * yDeltaOriginal);
            double angleToTranslation = translationAngle - previousState.position.Heading;
            double angleToFinal = this.position.Heading - translationAngle;
            
            fitness = double.NaN;

            if (applyNoise)
            {
                angleToTranslation += this.CalculateInitialAngleToTranslationAngleNoise(angleToTranslation);
                translationDelta += this.CalculateTranslationNoise(translationDelta);
                angleToFinal += this.CalculateTranslationAngleToFinalAngleNoise(angleToFinal);
            }

            double headingDelta = angleToTranslation + angleToFinal;
            double meanHeading = previousState.position.Heading + angleToTranslation;
            double xDelta = translationDelta * Math.Cos(meanHeading);
            double yDelta = translationDelta * Math.Sin(meanHeading);
            poseDelta = new Pose2D(xDelta, yDelta, headingDelta);
            return poseDelta;
        }

        /// <summary>
        /// Calculates a error distribution resulting from turning from previous heading to angle of translation in radians.
        /// </summary>
        /// <param name="angleToTranslationInRads">The amount of the rotation in radians</param>
        /// <returns>A Gaussian that contains the distribution</returns>
        private double CalculateInitialAngleToTranslationAngleNoise(double angleToTranslationInRads)
        {
            return
                new Gaussian(
                    0,
                    Math.Pow(this.noiseParams.AngleFromInitialToTranslationStdInRads, 2)
                    * Math.Abs(angleToTranslationInRads)).Sample();
        }

        /// <summary>
        /// Calculates a error distribution resulting from translation in the direction of motion
        /// </summary>
        /// <param name="translationalDistanceMeters">The distance of the translation</param>
        /// <returns>A Gaussian that contains the distribution</returns>
        private double CalculateTranslationNoise(double translationalDistanceMeters)
        {
            return
                new Gaussian(
                    0,
                    Math.Pow(this.noiseParams.TranslationStdDevInMeters, 2)
                    * Math.Abs(translationalDistanceMeters)).Sample();
        }

        /// <summary>
        /// Calculates a heading error distribution from a rotation
        /// </summary>
        /// <param name="rotationInRads">The size of the rotation in radians</param>
        /// <returns>A Gaussian that contains the distribution</returns>
        private double CalculateTranslationAngleToFinalAngleNoise(double rotationInRads)
        {
            return
                new Gaussian(
                    0,
                    Math.Pow(this.noiseParams.AngleFromTranslationToFinalStdInRads, 2)
                    * Math.Abs(rotationInRads)).Sample();
        }
    }
}
